#include "sharp_gp2.h"

// Measurement results for the infrared device
int IR_vals[ARRAY_SIZE(IR_IDs)];

int* get_IR_vals(){
	return IR_vals;
}

//Initialise AD-Converter and input pins
void init_IR_Sensors(){
	put_uart("[INIT] Infrared Sensors"); put_uart(CRLF);
	//reference voltage for ad-conversion
	IR_DDR&=~7;
	IR_PORT&=~7;

	ADMUX=(0<<REFS1)|(1<<REFS0)|(0<<ADLAR)|IR_1;
	//Conversion on, Auto Trigger on
	ADCSRA|=(1<<ADPS2)|(1<<ADPS1)|(1<<ADPS0)|(1<<ADEN)|(1<<ADSC);	//|(1<<ADATE);
	return;
}

/**
 * Reads the Infrared data from the given address
 */
unsigned int read_IR_Data(int id){
	unsigned int result=0xFFFF;

	ADMUX=(0<<REFS1)|(1<<REFS0)|(0<<ADLAR)|id;
	//Conversion on, Auto Trigger on
	ADCSRA|=(1<<ADPS2)|(1<<ADPS1)|(1<<ADEN)|(1<<ADSC);	//|(1<<ADATE)|(1<<ADPS0);

	while(!(ADCSRA& (1<<ADIF)));

	result=ADCL|(ADCH<<8); // read HighByte at last, to force update!

	ADCSRA|=(0<<ADIF);

	return result;
}

/**
 * Reads all Infrared data described in the IR_IDs constant using the
 * the read_IR_Data(int) method
 */
void read_all_IR_Data(){
	if(DEBUG){put_uart("[READ] IR Data"); put_uart(CRLF);}
	for(i=0;i<ARRAY_SIZE(IR_IDs);i++){
		IR_vals[i] = read_IR_Data(IR_IDs[i]);
		//_delay_ms(5000);
	}
	return;
}
